#include "SensorDataProcessedInput.h"
#define COUNT_THRESH 0
#define COUNT_LIMIT 10
#include <glm/gtx/transform.hpp>

using namespace std;

extern void doFilter (IplImage* depthImage);

SensorDataProcessedInput::SensorDataProcessedInput (SensorDataInput *origInput)
		: SensorDataInput (origInput->getName () + " Processed"),
		_origInput (origInput), _tempDepthImage (NULL)
{
	_calibration = new PatternCalibrationProcess (this);
	if (origInput->getRGBImage ())
		__rgbMetaData = cvCreateImage (cvGetSize (origInput->getRGBImage ()),
				8, 3);
	if (origInput->getDepthImage ())
		__depthMetaData = cvCreateImage (cvGetSize (origInput->getDepthImage ()),
				IPL_DEPTH_16U, 1);
}

SensorDataProcessedInput::~SensorDataProcessedInput ()
{
	delete _origInput;
}

bool SensorDataProcessedInput::grabFrame ()
{
	if (!_origInput->grabFrame () || !_origInput->getDepthImage ())
		return false;

	if (!_tempDepthImage)
		_tempDepthImage = cvCreateImage (cvGetSize (_origInput->getDepthImage ()),
				_origInput->getDepthImage ()->depth, 1);
	doFilter (_tempDepthImage);
	_updateVoxel ();
	_generateDepthImage ();
	if (_origInput->getRGBImage ()) {
		_imageDenoise.process (__rgbMetaData);
	}
	return true;
}

SensorParameter& SensorDataProcessedInput::getSensorParameter ()
{
	return _origInput->getSensorParameter ();
}

const SensorParameter& SensorDataProcessedInput::getSensorParameter () const
{
	return _origInput->getSensorParameter ();
}

void SensorDataProcessedInput::_updateVoxel ()
{
	unsigned int frameWidth = _origInput->getSensorParameter ().frameWidth;
	unsigned int frameHeight = _origInput->getSensorParameter ().frameHeight;
	unsigned short frameDepth = _origInput->getSensorParameter ().frameDepth;

	for (unsigned int x = 0; x < frameWidth; ++x) {
		for (unsigned int y = 0; y < frameHeight; ++y) {
			glm::dvec3 p (x, y, CV_IMAGE_ELEM (_tempDepthImage, unsigned short, y, x));
			bool used = false;
			for (DepthUnit::iterator iter = _depthUnits[y][x].begin ();
				iter != _depthUnits[y][x].end ();){
					if (abs (iter->first - p.z) < 20.0 && !used) {
						iter->first = (iter->first * iter->second + p.z) /
								(iter->second + 1);
						++iter->second;
						if (iter->second >= COUNT_LIMIT)
							iter->second = COUNT_LIMIT;
						used = true;
						++iter;
					} else {
						iter->second -= 1;
						if (iter->second <= 0)
							iter = _depthUnits[y][x].erase (iter);
						else
							++iter;
					}
			}
			if (!used)
				_depthUnits[y][x].push_back (make_pair (p.z, 1));
		}
	}
}

void SensorDataProcessedInput::_generateDepthImage ()
{
	for (int v = 0; v < 480; ++v) {
		for (int u = 0; u < 640; ++u) {
			int maxCount = -1;
			double maxCountZ = 10000.0;
			for (size_t i = 0; i < _depthUnits[v][u].size (); ++i) {
				if (_depthUnits[v][u][i].second >= COUNT_THRESH &&
						_depthUnits[v][u][i].second > maxCount) {
					maxCount = _depthUnits[v][u][i].second;
					maxCountZ = _depthUnits[v][u][i].first;
				}
			}
			CV_IMAGE_ELEM (__depthMetaData, unsigned short, v, u) = maxCountZ;
		}
	}
}

void SensorDataProcessedInput::calibration ()
{
	if (getRGBImage ()) {
		__extrinsicMat = _calibration->calculateExtrinsicMatrix ();
// 		if (getName () == "Sensor 1 Processed")
// 			__extrinsicMat = __extrinsicMat * glm::translate (-0.0f, 0.0f, 0.3f) *
// 			glm::rotate (1.0f, 0.0f, 1.0f, 0.0f) * glm::rotate (-1.0f, 1.0f, 0.0f, 0.0f);
		__inverseExtrinsicMat = glm::inverse (__extrinsicMat);
	}
}